#include "MoCapMarkerSensorPlot.h"

#include <boost/algorithm/string.hpp>

using namespace MMM;

MoCapMarkerSensorPlot::MoCapMarkerSensorPlot(MoCapMarkerSensorPtr sensor) :
    sensor(sensor)
{
    MMM::MoCapMarkerSensorMeasurementPtr measurement = sensor->getDerivedMeasurement(sensor->getMinTimestep());
    for (const auto &labeledMarker : measurement->getLabeledMarker()) {
        markerNames.push_back(labeledMarker.first + "_x");
        markerNames.push_back(labeledMarker.first + "_y");
        markerNames.push_back(labeledMarker.first + "_z");
    }
}

std::vector<std::string> MoCapMarkerSensorPlot::getNames() {
    return markerNames;
}

std::tuple<QVector<double>, QVector<double> > MoCapMarkerSensorPlot::getPlot(std::string name) {
    std::vector<std::string> results;
    boost::algorithm::split(results, name, boost::is_any_of("_"));
    std::string markerName = results[0];
    int id = ((int)results[1][0]) - ((int)'x');

    QVector<double> x(sensor->getTimesteps().size()), y(sensor->getTimesteps().size());
    for (unsigned int j = 0; j < sensor->getTimesteps().size(); j++) {
        float timestep = sensor->getTimesteps()[j];
        MMM::MoCapMarkerSensorMeasurementPtr measurement = sensor->getDerivedMeasurement(timestep);
        Eigen::Vector3f markerPosition = measurement->getLabeledMarker()[markerName];
        x[j] = timestep;
        y[j] = markerPosition(id);
    }
    return std::make_tuple(x, y);
}

std::string MoCapMarkerSensorPlot::getSensorName() {
    return sensor->getUniqueName();
}
